﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;
using Microsoft.Xna.Framework;

namespace SpaceFight.Units.Steering
{
	class Arrive : SteeringBehavior
	{
		private SteeringInfo.InfoType m_neededInfo = SteeringInfo.InfoType.Target            |
		                                             SteeringInfo.InfoType.MaxAcceleration   |
		                                             SteeringInfo.InfoType.MaxMoveSpeed      |
		                                             SteeringInfo.InfoType.ArriveTargetRadius|
		                                             SteeringInfo.InfoType.ArriveSlowRadius  |
		                                             SteeringInfo.InfoType.ArriveTimeToTarget;
		
		public override void GetSteering(IUnit p_unit, SteeringOutput p_steeringOutput)
		{
			if(HasValidInput(p_steeringOutput, p_unit, m_neededInfo) == false)
			{
				return;
			}
			
			// get the direction
			Vector3 direction = p_unit.SteeringInfo.Target.Position - p_unit.Position;
			float distance = direction.Length();
			
			// check if we arrived return no steering
			if(distance < p_unit.SteeringInfo.ArriveTargetRadius) return;
			
			float targetSpeed;
			
			// Outside the slow radius
			if(distance > p_unit.SteeringInfo.ArriveSlowRadius) 
			{
				// fully accelerate
				targetSpeed = p_unit.SteeringInfo.MaxMoveSpeed;
			}
			else
			{
				// calculate scaled speed
				targetSpeed = p_unit.SteeringInfo.MaxMoveSpeed * distance /
				              p_unit.SteeringInfo.ArriveSlowRadius;
				Debug.Assert(float.IsNaN(targetSpeed) == false);

			}
			
			// target velocity combines speed and direction
			Vector3 targetVelocity = direction;
			targetVelocity.Normalize();
			targetVelocity *= targetSpeed;
			
			// Acceleration tries to get to the target velocity
			p_steeringOutput.Linear = targetVelocity - p_unit.LinearVelocity;
			p_steeringOutput.Linear /= p_unit.SteeringInfo.ArriveTimeToTarget;
			Debug.Assert(float.IsNaN(p_steeringOutput.Linear.X) == false);
			Debug.Assert(float.IsNaN(p_steeringOutput.Linear.Y) == false);
			Debug.Assert(float.IsNaN(p_steeringOutput.Linear.Z) == false);
		}
	}
}
